// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template)
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <limits>
#include <vector>

#include "my_hardware_interface/my_hardware_interface.hpp"
#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "rclcpp/rclcpp.hpp"

namespace my_hardware_interface
{
  hardware_interface::CallbackReturn MyHardwareInterfac::on_init(
      const hardware_interface::HardwareInfo &info)
  {
    if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS)
    {
      return CallbackReturn::ERROR;
    }

    // TODO(anyone): read parameters and initialize the hardware
    hw_states_.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());
    hw_commands_.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());
    hw_velocities_.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());
    return CallbackReturn::SUCCESS;
  }

  hardware_interface::CallbackReturn MyHardwareInterfac::on_configure(
      const rclcpp_lifecycle::State & /*previous_state*/)
  {
    // TODO(anyone): prepare the robot to be ready for read calls and write calls of some interfaces
    joint_state_.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());

    return CallbackReturn::SUCCESS;
  }

  std::vector<hardware_interface::StateInterface> MyHardwareInterfac::export_state_interfaces()
  {
    std::vector<hardware_interface::StateInterface> state_interfaces;
    for (size_t i = 0; i < info_.joints.size(); ++i)
    {
      hw_states_[i] = std::stod(info_.joints[i].state_interfaces[0].initial_value);

      state_interfaces.emplace_back(hardware_interface::StateInterface(
          // TODO(anyone): insert correct interfaces
          info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_states_[i]));
      state_interfaces.emplace_back(
          info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_velocities_[i]);
    }

    return state_interfaces;
  }

  std::vector<hardware_interface::CommandInterface> MyHardwareInterfac::export_command_interfaces()
  {
    std::vector<hardware_interface::CommandInterface> command_interfaces;
    for (size_t i = 0; i < info_.joints.size(); ++i)
    {
      command_interfaces.emplace_back(hardware_interface::CommandInterface(
          // TODO(anyone): insert correct interfaces
          info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_commands_[i]));
    }

    return command_interfaces;
  }

  hardware_interface::CallbackReturn MyHardwareInterfac::on_activate(
      const rclcpp_lifecycle::State & /*previous_state*/)
  {
    // TODO(anyone): prepare the robot to receive commands
    hw_commands_ = hw_states_;
    joint_state_ = hw_states_;
    return CallbackReturn::SUCCESS;
  }

  hardware_interface::CallbackReturn MyHardwareInterfac::on_deactivate(
      const rclcpp_lifecycle::State & /*previous_state*/)
  {
    // TODO(anyone): prepare the robot to stop receiving commands

    return CallbackReturn::SUCCESS;
  }

  hardware_interface::return_type MyHardwareInterfac::read(
      const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/)
  {
    // TODO(anyone): read robot states
    hw_states_ = joint_state_;
    for (auto i = 0ul; i < hw_states_.size(); i++)
    {
      RCLCPP_INFO(rclcpp::get_logger("MyHardware"), "模拟实际关节返回量 : hw_states_ : %f", hw_states_[i]);
    }
    // 伪速度计算（实际需根据硬件实现）
    for (size_t i = 0; i < hw_velocities_.size(); ++i)
    {
      hw_velocities_[i] = 0.0; // 暂时用0代替
    }
    return hardware_interface::return_type::OK;
  }

  hardware_interface::return_type MyHardwareInterfac::write(
      const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/)
  {
    // TODO(anyone): write robot's commands'
    joint_state_ = hw_commands_;
    for (auto i = 0ul; i < hw_commands_.size(); i++)
    {
      RCLCPP_INFO(rclcpp::get_logger("MyHardware"), "模拟写入实际关节角 : hw_commands_ : %f", hw_commands_[i]);
    }
    return hardware_interface::return_type::OK;
  }

} // namespace my_hardware_interface

#include "pluginlib/class_list_macros.hpp"

PLUGINLIB_EXPORT_CLASS(
    my_hardware_interface::MyHardwareInterfac, hardware_interface::SystemInterface)
